# -*- coding: UTF-8 -*-
#!/usr/bin/env python
import sys   #reload()之前必须要引入模块
import time   
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist

twist=Twist()
cmd_type = 0
control_speed = 0
control_turn = 0

def iattextCallback(msg):
    global cmd_type,control_speed,control_turn
    print(type(msg.data),msg.data,msg.data)

    if "Move" in msg.data :
        cmd_type = 1
    elif "Back" in msg.data :
        cmd_type = 2
    elif "Left" in msg.data :
        cmd_type = 3
    elif "Right" in msg.data :
        cmd_type = 4
    elif "Stop" in msg.data :
        cmd_type = 5
    print(cmd_type)
    rate = rospy.Rate(10) 
    index=10
    while index:
        index-=1
        if cmd_type==1:
            control_speed = 0.1
            control_turn = 0.0
            break
        elif cmd_type==2:
            control_speed = -0.1
            control_turn = 0.0
            break
        elif cmd_type==3:
            control_speed = 0.1
            control_turn = 0.5
            break   
        elif cmd_type==4:
            control_speed = 0.1
            control_turn = -0.5
            break
        elif cmd_type==5:
            control_speed = 0.0
            control_turn = 0.0
            break    
        twist.linear.x = control_speed
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0 
        twist.angular.z = control_turn
        cmd_vel_pub.publish(twist)
        print(twist.linear.x," ",twist.angular.z)
        rate.sleep()


if __name__ == '__main__':
    rospy.init_node('voice_control', anonymous=True)
    iat_text_sub =rospy.Subscriber("iat_text", String,iattextCallback)
    cmd_vel_pub =rospy.Publisher("cmd_vel",Twist ,queue_size=1)
    rospy.spin()


